# 黎卡提方程求解P和反馈增益F

import numpy as np
import math

def LQR_Gain(A,B,Q,R,S):
    n = A.shape[0]#行数:系统矩阵维度
    p = B.shape[1]#列数：输入矩阵维度
    P0 = S #系统终值代价权重矩阵系统终值代价权重矩阵
    max_iter = 200 #迭代次数
    P = np.zeros((n,n*max_iter)) #初始化矩阵P为0矩阵，用于存放计算得到的p[k]
    P[:,0:n] = P0 #初始化矩阵P
    P_k_min_1 = P0 #
    tol = 1e-3 #系统稳态误差阈值
    diff = math.inf #初始化系统反馈增益为无穷
    F_N_min_k = math.inf
    k = 1 #初始化系统迭代步
    F = None
    
    while diff > tol :
        F_N_min_k_pre = F_N_min_k
        F_N_min_k = np.linalg.inv(R +B.T@P_k_min_1@B)@B.T@P_k_min_1@A
        P_k = (A-B@F_N_min_k).T@P_k_min_1@(A-B*F_N_min_k)+(F_N_min_k).T*R*(F_N_min_k)+Q
        P[:,n*k-n:n*k] = P_k
        P_k_min_1 = P_k
        # diff = abs(max(F_N_min_k - F_N_min_k_pre))
        diff = abs(np.max(np.abs(F_N_min_k - F_N_min_k_pre)))
        k = k + 1
        F = F_N_min_k
        if k > max_iter: 
            print("Maximum Number of Iterations Exceeded")
            break
        
    print("No. of Interation is : ",k)
    return F

        
        
        


    
  



    
